#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
基本使用示例
演示如何使用 FrankaPosServo 进行基本的机器人控制
"""

import time
import numpy as np
from frankaservo import FrankaPosServo

def main():
    """基本使用示例"""
    
    # 初始化机器人控制器
    print("初始化 Franka 机器人控制器...")
    robot = FrankaPosServo(ip="172.16.0.2", dynamics_factor=0.5, goal_tolerance=0.001)
    
    # 获取当前位置
    current_pos = robot.robot_position
    print(f"当前机器人位置: {current_pos}")
    
    # 示例 1: 相对移动
    print("\n示例 1: 相对移动")
    print("向上移动 5cm...")
    up_cmd = robot.gen_rela_move_cmd([0, 0, 0.05], max_speed=0.01)
    robot.non_blocking_move(up_cmd)
    
    # 等待一段时间让机器人移动
    time.sleep(3)
    
    # 检查是否到达目标
    while not robot.is_cmd_finished(current_pos + np.array([0, 0, 0.05])):
        time.sleep(0.1)
    
    print("到达目标位置！")
    
    # 示例 2: 绝对移动
    print("\n示例 2: 绝对移动")
    print("回到原始位置...")
    return_cmd = robot.gen_abs_move_cmd(current_pos, max_speed=0.02)
    robot.blocking_move(return_cmd)
    
    print("回到原始位置完成！")
    
    # 示例 3: 复杂轨迹
    print("\n示例 3: 复杂轨迹")
    print("执行方形轨迹...")
    
    # 定义方形轨迹的四个点
    square_points = [
        [0.05, 0, 0],      # 右
        [0.05, 0.05, 0],   # 右上
        [0, 0.05, 0],      # 上
        [0, 0, 0]          # 回到起点
    ]
    
    for i, point in enumerate(square_points):
        print(f"移动到点 {i+1}: {point}")
        cmd = robot.gen_rela_move_cmd(point, max_speed=0.015)
        robot.blocking_move(cmd)
        time.sleep(0.5)  # 在每个点停留一会儿
    
    print("方形轨迹完成！")
    
    # 停止机器人
    robot.stop_robot()
    print("\n示例完成，机器人已停止。")

if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print("\n用户中断程序")
    except Exception as e:
        print(f"\n程序执行出错: {e}")
        import traceback
        traceback.print_exc()
